#include "Terrain.h"
#include <assert.h>
#include "../tools/Log.h"
#include "../tools/CollisionDetection.h"

extern const unsigned long SUBSPACE_RESOLUTION;

Terrain::~Terrain() {}

void Terrain::setTerrain(terrainVec* t) {
	if(t == NULL || t->empty() || (*t)[0].empty())
		return;
		
	terrain = t;
	
	pads = PadGrid(t->size()-1, vector <Pad> ((*t)[0].size()-1));
	
	for(unsigned i = 0; i < t->size()-1; i++)
		for(unsigned j = 0; j < (*t)[i].size()-1; j++) {
			pads[i][j].set(i * SUBSPACE_RESOLUTION, j * SUBSPACE_RESOLUTION, &(*t)[i][j], &(*t)[i][j+1], &(*t)[i+1][j+1], &(*t)[i+1][j]);
		}
		
	EnableDraw();
}

bool Terrain::traceRayToMap(const fVec3D& p, const fVec3D& dir, lVec3D& winner) {
	vector<fVec3D> intersectingPoints;
	
	for(unsigned i = 0; i < pads.size(); i++)
		for(unsigned j = 0; j < pads[i].size(); j++) {
			lVec3D intersect;
			if(pads[i][j].testIntersection(p, dir, intersect))
				intersectingPoints.push_back(convert(intersect));
		}
	if(intersectingPoints.empty())	return false;
	
	double min = -1;
	for(unsigned i = 0; i < intersectingPoints.size(); i++) {
		double ans = (intersectingPoints[i] - p) | dir;
		if(min == -1 || ans < min) {
			winner = convert(intersectingPoints[i]);
			min = ans;
		}
	}
	
	return true;
}

unsigned long Terrain::getHeight(unsigned long X, unsigned long Y) const {
	unsigned x = X/SUBSPACE_RESOLUTION;
	unsigned y = Y/SUBSPACE_RESOLUTION;
	if(x > pads.size() || y > pads[0].size())
		return -1;
	
	return pads[x][y].getHeight(X, Y);
}

fVec3D Terrain::getNormal(unsigned long X, unsigned long Y) const {
	unsigned x = X/SUBSPACE_RESOLUTION;
	unsigned y = Y/SUBSPACE_RESOLUTION;
	if(x > pads.size() || y > pads[0].size())
		return -1;
		
	return pads[x][y].getNormal(X, Y);
}

fVec3D Pad::getNormal(unsigned long X, unsigned long Y) const {
	fVec3D n;
	fVec2D p(X - x, Y - y);
	
	fVec3D bd = convert(getPointD() - getPointB());
	if(p.y + p.x > SUBSPACE_RESOLUTION) {
		fVec3D bc = convert(getPointC() - getPointB());
		n = bd ^ bc;
	} else {
		fVec3D ba = convert(getPointA() - getPointB());
		n = bd ^ ba;
	}
	if(n.z < 0)
		n *= -1;
	if(n.x == -0)	n.x = 0;
	if(n.y == -0)	n.y = 0;
	if(n.z == -0)	n.z = 0;
	return n.normalized();
}
/*
fVec2D Pad::getGradient(unsigned long X, unsigned long Y) const {
	fVec2D grad;
	fVec2D p(X - x, Y - y);
	
	if(p.y + p.x > SUBSPACE_RESOLUTION) {	//upper triangle
//		lout<<"upper A="<<*A<<" B="<<*B<<" C="<<*C<<" D="<<*D<<endl;
		grad.x = std::atan(*D - *C);
		grad.y = std::atan(*B - *C);
	} else {		//lower triangle
//		lout<<"lower A="<<*A<<" B="<<*B<<" C="<<*C<<" D="<<*D<<endl;
		grad.x = std::atan(*B - *A);
		grad.y = -std::atan(*D - *A);
	}
	grad *= 180 / 3.1415926;
//	lout<<"grad = "<<grad.x<<","<<grad.y<<endl;
	return grad;
}
*/
unsigned long Pad::getHeight(unsigned long X, unsigned long Y) const {
	fVec3D orig(X, Y, SUBSPACE_RESOLUTION * SUBSPACE_RESOLUTION);
	fVec3D dir(0,0,-1);
	lVec3D intersection;
	
	testIntersection(orig, dir, intersection);
	
	return intersection.z;
}

bool Pad::testIntersection(const fVec3D& orig, const fVec3D& dir, lVec3D& intersection) const {
	if(RayTriangleCollision(orig, dir, getPointA(), getPointB(), getPointD(), intersection))
		return true;
	return RayTriangleCollision(orig, dir, getPointB(), getPointD(), getPointC(), intersection);
}

void Pad::draw() {
	if(A == NULL || B == NULL || C == NULL || D == NULL)	return;
	glPushMatrix();
		glTranslated(double(x), double(y), 0);
		glBegin(GL_QUADS);
			GET_COLOR(*A);		glVertex3d(0,0, *A * SUBSPACE_RESOLUTION);
			GET_COLOR(*B);		glVertex3d(0, SUBSPACE_RESOLUTION, *B * SUBSPACE_RESOLUTION);
			GET_COLOR(*C);		glVertex3d(SUBSPACE_RESOLUTION, SUBSPACE_RESOLUTION, *C * SUBSPACE_RESOLUTION);
			GET_COLOR(*D);		glVertex3d(SUBSPACE_RESOLUTION, 0, *D * SUBSPACE_RESOLUTION);
		glEnd();
	glPopMatrix();
}

lVec3D Pad::getPointA() const	{
	assert(A != NULL);	
	return lVec3D(x, y, *A * SUBSPACE_RESOLUTION);
}
lVec3D Pad::getPointB() const	{ 
	assert(B != NULL);	
	return lVec3D(x, y+SUBSPACE_RESOLUTION, *B * SUBSPACE_RESOLUTION);
}
lVec3D Pad::getPointC() const	{ 
	assert(C != NULL);	
	return lVec3D(x+SUBSPACE_RESOLUTION, y+SUBSPACE_RESOLUTION, *C * SUBSPACE_RESOLUTION);
}
lVec3D Pad::getPointD() const	{ 
	assert(D != NULL);	
	return lVec3D(x+SUBSPACE_RESOLUTION, y, *D * SUBSPACE_RESOLUTION);
}

void Terrain::draw(unsigned long) {
	if(terrain == NULL)	return;
	if(terrain->empty() || terrain->front().empty())
		return;
		
	for(PadGrid::iterator itr = pads.begin(); itr != pads.end(); itr++)
		for(vector<Pad>::iterator i = itr->begin(); i != itr->end(); i++)
			i->draw();
	
	glScaled(SUBSPACE_RESOLUTION,SUBSPACE_RESOLUTION,SUBSPACE_RESOLUTION);
	glBegin(GL_QUADS);

		glColor4d(0.3, 0.3, 1, 0.5);
		glVertex3d(0, 0, 0.99);
		glVertex3d(0, (*terrain)[0].size()-1, 0.99);
		glVertex3d(terrain->size()-1, (*terrain)[0].size()-1, 0.99);
		glVertex3d(terrain->size()-1, 0, 0.99);
	
	glEnd();

}
